#include <iostream>
#include <iomanip>
#include <sstream>
#include <vector>
#include <string>
#include <serial/serial.h>
#include <ros/ros.h>
#include <lk-motor.h>

#include "CANFrame.h"
#include "mi-motor.h"

using namespace std;

std::vector<unsigned char> CAN::CreateHexPack(const string &dataStr)
{
    std::vector<unsigned char> bytes;
    for (size_t i = 0; i < dataStr.size(); i += 2)
    {
        string byteString = dataStr.substr(i, 2);
        auto byte = static_cast<unsigned char>(stoi(byteString, nullptr, 16));
        bytes.push_back(byte);
    }
    return bytes;
}

std::string CAN::SerialDecode(const std::string &dataStr)
{
    stringstream outData;
    outData << hex << uppercase;
    for (char a: dataStr)
    {
        outData << setw(2) << setfill('0') << static_cast<int>(static_cast<unsigned char >(a));
    }
    return outData.str();
}

std::string CAN::extractFrame(const std::string &data)
{
    const std::string startDelimiter = "4154";
    const std::string endDelimiter = "0D0A";

    // 查找帧的起始位置
    size_t startPos = data.find(startDelimiter);
    if (startPos == std::string::npos)
    {
        // 如果找不到起始标记，则返回空字符串
        return "";
    }

    // 查找帧的结束位置
    size_t endPos = data.find(endDelimiter, startPos);
    if (endPos == std::string::npos)
    {
        // 如果找不到结束标记，则返回空字符串
        return "";
    }

    // 计算结束标记的长度
    size_t endDelimiterLength = endDelimiter.length();

    // 提取并返回帧
    return data.substr(startPos, endPos - startPos + endDelimiterLength);
}

uint32_t CAN::CreateCanExtendedId(uint8_t id, uint16_t torque, uint8_t mode)
{
//// mode 1 运控模式
//// mode 2 电机回复
//// mode 3 电机使能
//// mode 4 电机停止
    uint32_t extendedFrameId = 0;

    extendedFrameId |= (static_cast<uint32_t>(mode) & 0x1F) << 24;

    extendedFrameId |= (static_cast<uint32_t>(torque) & 0xFFFF) << 8;

    extendedFrameId |= static_cast<uint32_t>(id) & 0xFF;

    return extendedFrameId;
}

std::vector<unsigned char> CAN::CreateCanStandardFrame(uint32_t id, const std::vector<string> &datas)
{
    string FrameString;
////  帧头
    FrameString += "4154";
////  id
    uint32_t can_id = (id << 5) & 0xFFFF;
    stringstream ss;
    ss << hex << can_id;
    uint8_t countNum = 8 - ss.str().size();
    string zeros(countNum, '0');
    FrameString += ss.str();
    FrameString += zeros;
////  dlc
    FrameString += "08";
////  data
    for (const auto &data: datas)
        FrameString += data;
////  帧尾
    FrameString += "0D0A";

////  转hex
    std::vector<unsigned char> Frame;
    Frame = CreateHexPack(FrameString);

    return Frame;
}

std::vector<unsigned char> CAN::CreateCanExtendedFrame(uint32_t id, const vector<std::string> &datas)
{
    string FrameString;
////  帧头
    FrameString += "4154";
////  id
    uint32_t can_id = ((id << 3) + 0x4) & 0xFFFFFFFF;
    stringstream ss;
    ss << hex << can_id;
    uint8_t countNum = 8 - ss.str().size();
    string zeros(countNum, '0');
    FrameString += zeros;
    FrameString += ss.str();
////  dlc
    FrameString += "08";
////  data
    for (const auto &data: datas)
        FrameString += data;
////  帧尾
    FrameString += "0D0A";

////  转hex
    std::vector<unsigned char> Frame;
    Frame = CreateHexPack(FrameString);

    return Frame;
}

CAN::CANFrame CAN::ReadStandardFrame(const std::string &dataStr)
{
    string strDecode = SerialDecode(dataStr);
    vector<string> strFinal;
    for (int i = 0; i < strDecode.size(); i++)
    {
        if (strDecode[i] == '4' && strDecode[i + 1] == '1' && strDecode[i + 2] == '5' && strDecode[i + 3] == '4')
        {
            if (strDecode.size() - i >= 34)
            {
                if (strDecode[i + 30] == '0' && strDecode[i + 31] == 'D' && strDecode[i + 32] == '0'
                    && strDecode[i + 33] == 'A')
                {
                    strFinal.push_back(strDecode.substr(i, 34));
                }
            }
        }
    }
    CAN::CANFrame frame{};

    for (const auto &str: strFinal)
    {
        if (str.size() == 34)
        {
////  存id
            string id = str.substr(4, 4);
            uint32_t intValue;
            stringstream hexStream;
            hexStream << hex << id;
            hexStream >> intValue;
            frame.id = (intValue >> 5) & 0xFFFF;

////  存数据
            for (int i = 0; i < 8; i++)
            {
                string data = str.substr(i * 2 + 14, 2);
                int Value;
                Value = stoi(data, 0, 16);
                frame.data[i] = Value;
            }

        }
        if (frame.id == 321)
        {
            int speedLeft = static_cast<uint8_t>(frame.data[5]) << 8 | static_cast<uint8_t>(frame.data[4]);
            int encoderLeft = static_cast<uint8_t>(frame.data[7]) << 8 | static_cast<uint8_t>(frame.data[6]);

            if (speedLeft >= 32768)
            {
                speedLeft = speedLeft - 65535;
            }

            lkMotor.leftLK.speed = speedLeft;
            lkMotor.leftLK.location = encoderLeft;

        }
        if (frame.id == 322)
        {
            int speedRight = static_cast<uint8_t>(frame.data[5]) << 8 | static_cast<uint8_t>(frame.data[4]);
            int encoderRight = static_cast<uint8_t>(frame.data[7]) << 8 | static_cast<uint8_t>(frame.data[6]);

            if (speedRight >= 32768)
            {
                speedRight = speedRight - 65535;
            }

            lkMotor.rightLK.speed = speedRight;
            lkMotor.rightLK.location = encoderRight;
        }
    }
    return frame;
}

CAN::CANFrame CAN::ReadExtendedFrame(const std::string &dataStr)
{
    string strDecode = SerialDecode(dataStr);
    vector<string> strFinal;
    for (int i = 0; i < strDecode.size(); i++)
    {
        if (strDecode[i] == '4' && strDecode[i + 1] == '1' && strDecode[i + 2] == '5' && strDecode[i + 3] == '4')
        {
            if (strDecode.size() - i >= 34)
            {
                if (strDecode[i + 30] == '0' && strDecode[i + 31] == 'D' && strDecode[i + 32] == '0'
                    && strDecode[i + 33] == 'A')
                {
                    strFinal.push_back(strDecode.substr(i, 34));
                }
            }
        }
    }
    CAN::CANFrame frame{};

    for (const auto &str: strFinal)
    {
        if (str.size() == 34)
        {
////  存id
            string id = str.substr(8, 4);
            uint32_t intValue;
            stringstream hexStream;
            hexStream << hex << id;
            hexStream >> intValue;
            frame.id = intValue & 0xFFFF;

////  存数据
            for (int i = 0; i < 8; i++)
            {
                string data = str.substr(i * 2 + 14, 2);
                int Value;
                Value = stoi(data, 0, 16);
                frame.data[i] = Value;
            }

        }

        if (frame.id == 4100)
        {
            int speedLeft = static_cast<uint8_t>(frame.data[2]) << 8 | static_cast<uint8_t>(frame.data[3]);
            int encoderLeft = static_cast<uint8_t>(frame.data[0]) << 8 | static_cast<uint8_t>(frame.data[1]);
            int torqueLeft = static_cast<uint8_t>(frame.data[4]) << 8 | static_cast<uint8_t>(frame.data[5]);

            miMotor.leftMi.angularVelocity = speedLeft;
            miMotor.leftMi.angle = encoderLeft;
            miMotor.leftMi.torque = float(torqueLeft - 32767) / 32768 * 12;
        }
        if (frame.id == 2052)
        {
            int speedRight = static_cast<uint8_t>(frame.data[2]) << 8 | static_cast<uint8_t>(frame.data[3]);
            int encoderRight = static_cast<uint8_t>(frame.data[0]) << 8 | static_cast<uint8_t>(frame.data[1]);
            int torqueRight = static_cast<uint8_t>(frame.data[4]) << 8 | static_cast<uint8_t>(frame.data[5]);

            miMotor.rightMi.angularVelocity = speedRight;
            miMotor.rightMi.angle = encoderRight;
            miMotor.rightMi.torque = float(torqueRight - 32767) / 32768 * 12;
        }
    }
    return frame;
}

